//
// Created by ze on 2024/1/3.
//
#include "UPRE_Hand_Shank.h"
#include "usart.h"
#include "UPRE_PID.h"

//手柄数据
Hand_Shank_ Hand_Shank_Data = {0};

//蓝牙接受函数
void Start_Hand_Shank_Receive(void)
{
//    printf("l_x:%d\t l_y:%d\t r_x:%d\t r_y:%d \r\n",Hand_Shank_Data.left_x,Hand_Shank_Data.left_y,Hand_Shank_Data.right_x,Hand_Shank_Data.right_y);
//    printf("dir:%f\t  vel:%f\t Yaw:%f\r\n",Hand_Shank_Data.Vel_Hand_Shank.dir,Hand_Shank_Data.Vel_Hand_Shank.size,Hand_Shank_Data.Pose_Hand_Shank.yaw);
    /**********************************************计算校验和******************************************************/
    if(Hand_Shank_Data.Hand_Shank_Recv[27] == Hand_Shank_Data.sum)
    {
//        printf("%d\r\n",Hand_Shank_Data.Flag_Start_HandShank);
//        HAL_GPIO_TogglePin(LED1_GPIO_Port,LED1_Pin);
        Hand_Shank_Data.A =      Hand_Shank_Data.Hand_Shank_Recv[1];
        Hand_Shank_Data.B =      Hand_Shank_Data.Hand_Shank_Recv[2];
        Hand_Shank_Data.C =      Hand_Shank_Data.Hand_Shank_Recv[3];
        Hand_Shank_Data.D =      Hand_Shank_Data.Hand_Shank_Recv[4];
        Hand_Shank_Data.E =      Hand_Shank_Data.Hand_Shank_Recv[5];
        Hand_Shank_Data.F =      Hand_Shank_Data.Hand_Shank_Recv[6];
        Hand_Shank_Data.G =      Hand_Shank_Data.Hand_Shank_Recv[7];
        Hand_Shank_Data.H =      Hand_Shank_Data.Hand_Shank_Recv[8];
        Hand_Shank_Data._2 =     Hand_Shank_Data.Hand_Shank_Recv[9];
        Hand_Shank_Data._3 =     Hand_Shank_Data.Hand_Shank_Recv[10];
        Hand_Shank_Data._4 =     Hand_Shank_Data.Hand_Shank_Recv[11];
        Hand_Shank_Data.R =      Hand_Shank_Data.Hand_Shank_Recv[12];
        Hand_Shank_Data.Add =    Hand_Shank_Data.Hand_Shank_Recv[13];
        Hand_Shank_Data.Minus =  Hand_Shank_Data.Hand_Shank_Recv[14];
        Hand_Shank_Data.LEFT =   Hand_Shank_Data.Hand_Shank_Recv[15];
        Hand_Shank_Data._1=      Hand_Shank_Data.Hand_Shank_Recv[16];
        Hand_Shank_Data.RIGHT =  Hand_Shank_Data.Hand_Shank_Recv[17];
        Hand_Shank_Data.L =      Hand_Shank_Data.Hand_Shank_Recv[18];

            Hand_Shank_Data.left_x = (short)(Hand_Shank_Data.Hand_Shank_Recv[19]) | (short)(Hand_Shank_Data.Hand_Shank_Recv[20]<<8);
            Hand_Shank_Data.left_x -= 1985;
            Hand_Shank_Data.left_x = Hand_Shank_Data.left_x * 0.1;

            Hand_Shank_Data.left_y = (short)(Hand_Shank_Data.Hand_Shank_Recv[21]) | (short)(Hand_Shank_Data.Hand_Shank_Recv[22]<<8);
            Hand_Shank_Data.left_y -= 1800;
            Hand_Shank_Data.left_y = Hand_Shank_Data.left_y * 0.1;

            Hand_Shank_Data.right_x = (short)(Hand_Shank_Data.Hand_Shank_Recv[23]) | (short)(Hand_Shank_Data.Hand_Shank_Recv[24]<<8);
            Hand_Shank_Data.right_x -= 1950;
            Hand_Shank_Data.right_x = Hand_Shank_Data.right_x * 0.01;

            Hand_Shank_Data.right_y = (short)(Hand_Shank_Data.Hand_Shank_Recv[25]) | (short)(Hand_Shank_Data.Hand_Shank_Recv[26]<<8);
            Hand_Shank_Data.right_y -= 1890;
            Hand_Shank_Data.right_y = Hand_Shank_Data.right_y * 0.01;

            Hand_Shank_Data.Vel_Hand_Shank.dir = Radian2Angle(atan2f(Hand_Shank_Data.left_x,Hand_Shank_Data.left_y));
            Hand_Shank_Data.Vel_Hand_Shank.size = (float)(fabsf((float)Hand_Shank_Data.left_x) + fabsf((float)Hand_Shank_Data.left_y)) * 10.f;
            Hand_Shank_Data.Pose_Hand_Shank.yaw = Radian2Angle(atan2f(Hand_Shank_Data.right_x,Hand_Shank_Data.right_y));

            setPidCalMode(PID_YAW_ONLY);
            setPidTargetPose(&Hand_Shank_Data.Pose_Hand_Shank,PID_YAW_ONLY);


    }


    /************************************************************************************************************/

}
